import serial
import os
import time


# Connect to arudino, ping and print result
def main():
	usbpath = ""
	reply = ""

	try:
		print "Starting program..."

		for file in os.listdir("/dev"):
		    if "tty.usb" in file.lower():
				usbpath = os.path.join("/dev", file)
				print "USB connection found at: ", usbpath

		if usbpath == "":
			print "Could not find USB connection. Exiting..."
			return
		else:
			ser = serial.Serial(port=usbpath, baudrate=9600, timeout=3)
			time.sleep(0.1)

			# Check connection
			ser.write("p")
			reply = ser.readline()
			if reply[0] == "p":
				print "Connection Established"

			# Send coordinate
			ser.write("s 0 360 0 90")

			while True:
				reply = ser.readline()
				
				if reply[0] == "c":
					print "Coordinate received"
					print reply
				if reply[0] == "x":
					print "Exiting..."
					ser.close()
					return
	
	except Exception, e:
		raise

	else:
		pass

	finally:
		pass


main()